﻿using System;
using System.Collections.Generic;
using System.Text;
using RobotRace.utils.graph;
using RobotRace.Geometry;

namespace RobotRace.engine.path
{
    public class ClosestCheckpointManagementStrategy : CheckpointManagementStrategy
    {
        private Node currentNode = null;

        public Node findNextPoint(PathGraph graph, Point2D robotPosition)
        {
            if (graph.Count < 1)
            {
                currentNode = graph.getFirstNodeInSequence();
                return graph.getFirstNodeInSequence();
            }
            int newCurrentUsedCheckpoint = getCurrentCheckpoint(graph).nodeNumberOnPath;
            newCurrentUsedCheckpoint = GetClosestPointInFrontOfRobot(robotPosition, graph, newCurrentUsedCheckpoint);
            if (isCurrentCheckpointInFrontOfRobot(robotPosition, graph, newCurrentUsedCheckpoint))
            {
                newCurrentUsedCheckpoint = getNextCheckpointFromPath(graph, newCurrentUsedCheckpoint);
                //if reached checkpoint number 0 and lap is completed
                /*if (newCurrentUsedCheckpoint == 0)
                {
                    DateTime stopTime = DateTime.Now;
                    if (validLap == true)
                    {
                        lastLap = stopTime - lastLapStartTime;
                        engine.registerLapTime(lastLap);
                    }
                    validLap = true;
                    lastLapStartTime = DateTime.Now;
                }*/
            }
            currentNode = graph.getNodeWithIdInSequence(newCurrentUsedCheckpoint);
            return currentNode;
        }

        public Node getCurrentCheckpoint(PathGraph graph)
        {
            if (currentNode == null)
            {
                currentNode = graph.getFirstNodeInSequence();
            }
            return currentNode;
        }

        private int GetClosestPointInFrontOfRobot(Point2D robotPosition, PathGraph path, int newCurrentUsedCheckpoint)
        {
            List<double> distances = new List<double>();
            foreach (Node pointNode in path.nodes)
            {
                distances.Add(pointNode.point.GetDistance(robotPosition.x, robotPosition.y));
            }
            int nextCheckpoint = newCurrentUsedCheckpoint;
            do
            {
                newCurrentUsedCheckpoint = nextCheckpoint;
                nextCheckpoint = getNextCheckpointFromPath(path, newCurrentUsedCheckpoint);
            } while (distances[newCurrentUsedCheckpoint] >= distances[nextCheckpoint]);
            return newCurrentUsedCheckpoint;
        }

        private bool isCurrentCheckpointInFrontOfRobot(Point2D robotPosition, PathGraph path, int currentUsedCheckpoint)
        {
            int prevCheckpoint = getPreviousCheckpointFromPath(path, currentUsedCheckpoint);
            int nextCheckpoint = getNextCheckpointFromPath(path, currentUsedCheckpoint);
            double prevDistPointToPoint = path.getNodeWithIdInSequence(prevCheckpoint).point.GetDistance(path.getNodeWithIdInSequence(currentUsedCheckpoint).point);
            double nextDistPointToPoint = path.getNodeWithIdInSequence(nextCheckpoint).point.GetDistance(path.getNodeWithIdInSequence(currentUsedCheckpoint).point);
            double prevDistRobotToPoint = path.getNodeWithIdInSequence(prevCheckpoint).point.GetDistance(robotPosition);
            double nextDistRobotToPoint = path.getNodeWithIdInSequence(nextCheckpoint).point.GetDistance(robotPosition);
            if ((prevDistRobotToPoint / nextDistRobotToPoint) > (prevDistPointToPoint / nextDistPointToPoint))
            {
                return true;
            }
            return false;
        }

        private int getNextCheckpointFromPath(PathGraph path, int currentUsedCheckpoint)
        {
            currentUsedCheckpoint += 1;
            if (currentUsedCheckpoint >= path.Count)
            {
                currentUsedCheckpoint = 0;
            }
            return currentUsedCheckpoint;
        }

        private int getPreviousCheckpointFromPath(PathGraph path, int currentUsedCheckpoint)
        {
            currentUsedCheckpoint -= 1;
            if (currentUsedCheckpoint < 0)
            {
                currentUsedCheckpoint = path.Count - 1;
            }
            return currentUsedCheckpoint;
        }
    }
}
